#include <signal.h>
#include <getopt.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include <linux/videodev2.h>
#include <libv4l2.h>
#include "host_uvc.h"

#define VIDEO_DEVICE "/dev/video0"
#define BUF_COUNT 4 


struct buffer {
    void *start;
    size_t length;
};

#define array_size(a)    (sizeof(a) / sizeof((a)[0]))

format_info g_pixel_formats[] = {
    { "YUYV", V4L2_PIX_FMT_YUYV, 1 },
    { "MJPEG", V4L2_PIX_FMT_MJPEG, 1 },
    { "H264", V4L2_PIX_FMT_H264, 1 },
    { "H265", V4L2_PIX_FMT_H265, 1 },
    { "NV12", V4L2_PIX_FMT_NV12, 1 },
    { "NV21", V4L2_PIX_FMT_NV21, 1 },
};

static struct option g_opts[] = {
    {"file", 2, 0, 'F'},
    {"format", 1, 0, 'f'},
    {"help", 0, 0, 'h'},
    {"size", 1, 0, 's'},
    {"numbers", 1, 0, 'n'},
    {0, 0, 0, 0}
};

static const format_info *sample_uvc_v4l2_format_by_fourcc(td_u32 fourcc)
{
    td_u32 i;

    for (i = 0; i < array_size(g_pixel_formats); ++i) {
        if (g_pixel_formats[i].fourcc == fourcc) {
            return &g_pixel_formats[i];
        }
    }

    return TD_NULL;
}

static td_void sample_uvc_usage(const td_char *argv0)
{
    printf("sample_uvc_usage: %s device [options]\n", argv0);
    printf("supported options:\n");
    printf("-f, --format format             set the video format\n");
    printf("-F, --file[=name]               write file\n");
    printf("-h, --help                      show help info\n");
    printf("-s, --size WxH                  set the frame size (eg. 1920x1080)\n");
    printf("-n, --frame numbers             capture frame numbers (eg. 1)\n\n");
    printf("inquire USB device format: ./sample_uvc  --enum-formats\n\n");
    printf("example of setting USB device format:\n");
    printf("    ./host_uvc_libv4l2  /dev/media0  -fH264  -s1920x1080 -Ftest.h264  -n1\n");
    printf("    ./host_uvc_libv4l2  /dev/media0  -fH265  -s1920x1080 -Ftest.h265  -n1\n");
    printf("    ./host_uvc_libv4l2  /dev/media0  -fMJPEG -s1920x1080 -Ftest.mjpg  -n1\n");
    printf("    ./host_uvc_libv4l2  /dev/media0  -fYUYV  -s1920x1080 -Ftest.yuv   -n1\n");
    printf("    ./host_uvc_libv4l2  /dev/media0  -fNV21  -s640x360   -Ftest.yuv   -n1\n\n");
    printf("note: set macro MEDIA_WORK to 0 to write file on disk.\n\n");
}

static const format_info *sample_uvc_v4l2_format_by_name(const td_char *name)
{
    td_u32 i;
    for (i = 0; i < array_size(g_pixel_formats); ++i) {
        //printf("***input for loop, i=%d***\n", i);
        //printf("g_pixel_formats =%s , name=%s\n", g_pixel_formats[i].name, name);
        //printf("g_pixel_formats =%d , name=%d \n", g_pixel_formats[i].name, name);
        //if (strcasecmp(g_pixel_formats[i].name, name) == 0) {
        if (!strcmp(g_pixel_formats[i].name, name)) {
            //printf("g_pixel_formats =%d \n", g_pixel_formats[i].name);
            return &g_pixel_formats[i];
            
        }       
    }
    return TD_NULL;
}

static td_void sample_uvc_list_formats(td_void)
{
    td_u32 i;
    for (i = 0; i < array_size(g_pixel_formats); i++) {
            printf("%s (\"%c%c%c%c\", %u planes)\n", g_pixel_formats[i].name, g_pixel_formats[i].fourcc & 0xff,
            (g_pixel_formats[i].fourcc >> 8) & 0xff, (g_pixel_formats[i].fourcc >> 16) & 0xff,  /* 8,16:shift */
            (g_pixel_formats[i].fourcc >> 24) & 0xff, g_pixel_formats[i].n_planes);             /* 24:shift */
    }
}

static td_s32 sample_uvc_get_opt(td_s32 argc, td_char *argv[], uvc_ctrl_info *ctrl_info)
{
    td_s32 c;
    const format_info *info;
    td_char *endptr;

    while ((c = getopt_long(argc, argv, "B:c::Cd:f:F::hi:Iln:p::q:r:R::s:t:uw:x:", g_opts, TD_NULL)) != -1) {
        switch (c) {
            case 'f':
                if (!strcmp("help", optarg)) {
                    sample_uvc_list_formats();
                    return TD_FAILURE;
                }
                printf("    -f:fromat=%s\n", optarg);
                ctrl_info->do_set_format = 1;
                info = sample_uvc_v4l2_format_by_name(optarg);
                //printf("info->fourcc=%d \n",info->fourcc);
                if (info == TD_NULL) {
                    printf("Unsupported video format '%s'\n", optarg);
                    return TD_FAILURE;
                }
                ctrl_info->pixelformat = info->fourcc;
                //printf("ctrl_info->pixelformat=%s \n",ctrl_info->pixelformat);
                break;
            case 'F':
                printf("    -F: pattern=%s\n", optarg);
                ctrl_info->do_file = 1;
                if (optarg)
                    ctrl_info->pattern = optarg;
                //printf("ctrl_info->pattern=%s \n",ctrl_info->pattern);
                break;
            case 'h':
                sample_uvc_usage(argv[0]);
                return TD_FAILURE;
            case 's':
                printf("    -s:size\n");      
                ctrl_info->do_capture = 1;
                ctrl_info->do_set_format = 1;
                ctrl_info->width = strtol(optarg, &endptr, 10); /* 10:base */
                if (*endptr != 'x' || endptr == optarg) {
                    printf("Invalid size '%s'\n", optarg);
                    return TD_FAILURE;
                }
                ctrl_info->height = strtol(endptr + 1, &endptr, 10);    /* 10:base */
                if (*endptr != 0) {
                    printf("Invalid size '%s'\n", optarg);
                    return TD_FAILURE;
                }
                break;
            case 'n':
                printf("    -n:frame numbers\n");      
                ctrl_info->nframes = strtol(optarg, &endptr, 10); /* 10:base */
                if (endptr == optarg) {
                    printf("Invalid size '%s'\n", optarg);
                    return TD_FAILURE;
                }
                break;
            default:
                printf("Invalid option -%c\n", c);
                printf("Run %s -h for help.\n", argv[0]);
                return TD_FAILURE;
        }
    }
    return TD_SUCCESS;
}

static td_void sample_uvc_ctrl_info_init(uvc_ctrl_info *ctrl_info)
{
    ctrl_info->nframes = 1;
    ctrl_info->pattern = "frame-#.bin";
    ctrl_info->width = 640;     /* 640:width */
    ctrl_info->height = 480;    /* 480:height */
    ctrl_info->pixelformat = V4L2_PIX_FMT_YUYV;
}

//const td_char *sample_uvc_v4l2_format_name(td_u32 fourcc)
//{
//    const format_info *format;
//    static td_char format_name[5];     /* 5: array len */
//    td_u32 i;
//    format = sample_uvc_v4l2_format_by_fourcc(fourcc);
//    if (format) {
//        return format->name;
//    }

//    for (i = 0; i < 4; ++i) {       /* 4: format */
//        format_name[i] = fourcc & 0xff;
//        fourcc >>= 8;               /* 8: shift */
//    }

//    format_name[4] = '\0';     /* 4: end */
//    return format_name;
//}

int main(int argc,char *argv[]) {
    int fd;
    struct v4l2_capability cap;
    struct v4l2_format fmt;
    struct v4l2_requestbuffers req;
    struct v4l2_buffer buf;
    struct buffer *buffers;
    unsigned int n_buffers;

    int ret;
    uvc_ctrl_info ctrl_info = {0};
    printf("--0-1-- host_uvc_ctrl_info_init\n");
    sample_uvc_ctrl_info_init(&ctrl_info);
    printf("--0-2-- host_uvc_get_opt\n");
    ret = sample_uvc_get_opt(argc, argv, &ctrl_info);
    if (ret != TD_SUCCESS) {
        return TD_FAILURE;
    }
    if (optind >= argc) {
        sample_uvc_usage(argv[0]);
        return TD_FAILURE;
    }
    // 1. open device
    fd = v4l2_open(VIDEO_DEVICE, O_RDWR | O_NONBLOCK, 0);
    if (fd < 0) {
        perror("Open device failed");
        return -1;
    }

    // 2. query device capabilities 
    if (v4l2_ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
        perror("Query capability failed");
        goto error;
    }
    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        fprintf(stderr, "Not a video capture device\n");
        goto error;
    }
    if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
        fprintf(stderr, "Device does not support streaming i/o\n");
        close(fd);
        return EXIT_FAILURE;
    } 

    // 3. set pic format
    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = ctrl_info.width;
    fmt.fmt.pix.height = ctrl_info.height;
    fmt.fmt.pix.pixelformat = ctrl_info.pixelformat;
    fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;

    if (v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
        perror("Set format failed");
        goto error;
    }

    // 4. request device buffer 
    memset(&req, 0, sizeof(req));
    req.count = BUF_COUNT;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    if (v4l2_ioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
        perror("Request buffers failed");
        goto error;
    }
    if (req.count < 2) {
        fprintf(stderr, "Insufficient buffer memory on device\n");
        close(fd);
        return EXIT_FAILURE;
    }

    buffers = calloc(req.count, sizeof(*buffers));
    //n_buffers = req.count;

    // 5. device buffer map to  user buffer 
    for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
        memset(&buf, 0, sizeof(buf));
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = n_buffers;

        if (v4l2_ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
            perror("Query buffer failed");
	        free(buffers);
            goto error;
        }

        buffers[n_buffers].length = buf.length;
        buffers[n_buffers].start = v4l2_mmap(NULL, buf.length, 
            PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
        
        if (buffers[n_buffers].start == MAP_FAILED) {
            perror("mmap failed");
	        free(buffers);
            goto error;
        }

	// put buffer to queue
	    if(v4l2_ioctl(fd, VIDIOC_QBUF, &buf) < 0) {
	        perror("Failed to queue buffer");
	        free(buffers);
            goto error;
	    }
    }

    // 6. start device  
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (v4l2_ioctl(fd, VIDIOC_STREAMON, &type) < 0) {
        perror("Start stream failed");
	    free(buffers);
        goto error;
    }

    // 7. capture 10 frame 
    for (int i = 0; i < ctrl_info.nframes; ++i) {
        fd_set fds;
        FD_ZERO(&fds);
        FD_SET(fd, &fds);

        struct timeval tv = {0};
        tv.tv_sec = 1;

        int r = select(fd + 1, &fds, NULL, NULL, &tv);
        if (-1 == r) {
            //if ( EINTR == errno){
            //    continue;
	        //}
            perror("Select");
        } else if (0 == r) {
            fprintf(stderr, "Select timeout\n");
        } else {
	        memset(&buf, 0, sizeof(buf));
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            buf.memory = V4L2_MEMORY_MMAP;

            // capture one frame  from queue
            if (v4l2_ioctl(fd, VIDIOC_DQBUF, &buf) < 0) {
                perror("Dequeue buffer failed");
                continue;
            }

            // deal stream  (save file)
            //char filename[32];
            //sprintf(filename, "frame_%03d.yuv",i);
            //FILE *fp = fopen(filename, "wb");
            //FILE *fp = fopen("frame.yuv", "ab+");
            FILE *fp = fopen(ctrl_info.pattern, "ab+");
            fwrite(buffers[buf.index].start, buf.bytesused, 1, fp);
            fclose(fp);

	        printf("Captured frame %d, size: %zu bytes\n", i, buf.bytesused);
            // Re-queue buffer 
            if (v4l2_ioctl(fd, VIDIOC_QBUF, &buf) < 0) {
                perror("Queue buffer failed");
            }
	    }
    }

    // 8. release 
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (v4l2_ioctl(fd, VIDIOC_STREAMOFF, &type) == -1) {
        perror("Failed to stop streaming");
    }
    for (unsigned int i = 0; i < n_buffers; ++i) {
        v4l2_munmap(buffers[i].start, buffers[i].length);
    }
    free(buffers);
    v4l2_close(fd);
    return 0;

error:
    if (fd >= 0) v4l2_close(fd);
    return -1;
}
